/* Servo motor PWM Programming For 
PIC 18f4431. */
#include<htc.h>

void Load_Servo();
void Load_Base();
void Load_Elbow();
void Load_Wrist();
void Load_R_Grip();
void Load_Grip();
void Load_Default();
void manual(void);
void delay(unsigned int);

unsigned char deg_servo  = 31;
unsigned char deg_base   = 20;
unsigned char deg_elbow  = 10;
unsigned char deg_wrist  = 31;
unsigned char deg_r_grip = 31;
unsigned char deg_grip   = 31;

/*----Look up table for CCP PWM---*/
unsigned const char reg_CON[4] = 
  { 0x0C, 0x1C, 0x2C, 0x3C };

unsigned const char reg_CCP[64] = 
  { 0x04, 0x04, 0x04, 0x04,
    0x05, 0x05, 0x05, 0x05,
    0x06, 0x06, 0x06, 0x06,
    0x07, 0x07, 0x07, 0x07,
    0x08, 0x08, 0x08, 0x08,
    0x09, 0x09, 0x09, 0x09,
    0x0A, 0x0A, 0x0A, 0x0A,
    0x0B, 0x0B, 0x0B, 0x0B,
    0x0C, 0x0C, 0x0C, 0x0C,
    0x0D, 0x0D, 0x0D, 0x0D,
    0x0E, 0x0E, 0x0E, 0x0E,
    0x0F, 0x0F, 0x0F, 0x0F,
    0x10, 0x10, 0x10, 0x10,
    0x11, 0x11, 0x11, 0x11,
    0x12, 0x12, 0x12, 0x12,
    0x13, 0x13, 0x13, 0x13 };
                                  
/*----Look up table for PCPWM-----*/
unsigned const char reg_PDCH[4] = 
  { 0x01, 0x02, 0x03, 0x04 };

unsigned const char reg_PDCL[16] = 
  { 0x00, 0x10, 0x20, 0x30,
    0x40, 0x50, 0x60, 0x70, 
    0x80, 0x90, 0xA0, 0xB0, 
    0xC0, 0xD0, 0xE0, 0xF0 };

void main()
{
/* Configures port as digital I/O */
  ANSEL0	= 0x00;		
  ANSEL1  = 0x00; 	

/* Configures the port pins as 
either In/Out */
  TRISA = 0x00; 	
  TRISB	= 0XC0; 	 
  TRISC	= 0xF0; 	
  TRISD	= 0x3F; 	
  TRISE	= 0x00; 	

/* Setting Internal Oscillator to 
run at 500KHZ. */
  OSCCON	= 0X3E; 	

/*------CCP module registers -----*/
/* For gentrating time period 20mS */
  PR2		= 0x9B;        
/* Configure Timer 2 */
  T2CON	= 0x06;        
  
/*--PCPWM control Module Registers-*/
  PWMCON0	= 0xDF;
  PTCON0	= 0x00;
  PTPERL	= 0xC4;
  PTPERH	= 0x09;
  PTMRL	  = 0xC4;
  PTMRH	  = 0x09;
  PTCON1	= 0x80;
  PORTA   = 0xFF;

  delay(1000);
/* Loads the default angles for each
 * joint */
  Load_Default();
/* Start polling the switches */
  manual();

/* Stops controller execution */
  while(1);	
}

/*------------Delay---------------*/

void delay(unsigned int count)
{
  unsigned int i;
  for(i=0;i<count;i++);
}                  
   
/*------Loads the base servo------*/
void Load_Servo()
{
 CCPR1L = reg_CCP[deg_servo];
 CCP1CON = reg_CON[deg_servo & 0x03];
}

/*---Loads the shoulder servo-----*/
void Load_Base()
{
  CCPR2L = reg_CCP[deg_base];
  CCP2CON = reg_CON[deg_base & 0x03];
}

/*------Loads the elbow servo-----*/
void Load_Elbow()
{
  PDC0H = reg_PDCH[deg_elbow >> 4];
  PDC0L = reg_PDCL[deg_elbow & 0x0F];
}

/*------Loads the wrist servo-----*/
void Load_Wrist()
{
  PDC1H = reg_PDCH[deg_wrist >> 4];
  PDC1L = reg_PDCL[deg_wrist & 0x0F];
}

/*--Loads the rotating grip servo--*/
void Load_R_Grip()
{
 PDC2H = reg_PDCH[deg_r_grip >> 4];
 PDC2L = reg_PDCL[deg_r_grip & 0x0F];
}

/*------Loads the grip servo------*/
void Load_Grip()
{
  PDC3H = reg_PDCH[deg_grip >> 4];
  PDC3L = reg_PDCL[deg_grip & 0x0F];
}

/* Loads the default servo joint 
angles */
void Load_Default()
{
  while(deg_base != 15)
  {
    if( deg_base < 15)
    {
      deg_base++;
      Load_Base();
      delay(500);
    }
    else
    {
      deg_base--;
      Load_Base();
      delay(500);
    }
  }
  Load_Base();

  while(deg_elbow != 5)
  {
    if( deg_elbow < 5)
    {
      deg_elbow++;
      Load_Elbow();
      delay(300);
    }
    else
    {
      deg_elbow--;
      Load_Elbow();
      delay(300);
    }
  }
  Load_Elbow();

  delay(1000);
  while(deg_servo != 31)
  {
    if( deg_servo < 31 )
    {
      deg_servo++;
      Load_Servo();
      delay(300);
    }
    else
    {
      deg_servo--;
      Load_Servo();
      delay(300);
    }
  }
  Load_Servo();

  while(deg_wrist != 10)
  {
    if( deg_wrist < 10)
    {
      deg_wrist++;
      Load_Wrist();
      delay(300);
    }
    else
    {
      deg_wrist--;
      Load_Wrist();
      delay(300);
    }
  }
  Load_Wrist();

  while(deg_r_grip != 31)
  {
    if( deg_r_grip < 31)
    {
      deg_r_grip++;
      Load_R_Grip();
      delay(300);
    }
    else
    {
      deg_r_grip--;
      Load_R_Grip();
      delay(300);
    }
  }    
  Load_R_Grip();
}

/* Polls switches and loads values
for manual control */

void manual(void)
{
  while(1)
  {
    /*----------Base servo--------*/
    if((RD5 == 1)&&(deg_servo < 63))
    {
      deg_servo++;
      Load_Servo();
      delay(300);
    }
    else if((RD4 == 1)&&
      (deg_servo> 0))
    {
      deg_servo--;
      Load_Servo();
      delay(300);
    }
    /*--------Shoulder servo------*/
    if((RC7 == 1)&&
      (deg_base < 63))
    {
      deg_base++;
      Load_Base();
      delay(500);
    }
    else if((RD6 == 1)&&
      (deg_base > 13 ))
    {
      deg_base--;
      Load_Base();
      delay(500);
    }
    /*--------Elbow servo---------*/

    if((RC5 == 1)&&(deg_elbow < 63))
    {
      deg_elbow++;
      Load_Elbow();
      delay(500);
    }
    else if((RC4 == 1)&&
      (deg_elbow > 0))
    {
      deg_elbow--;
      Load_Elbow();
      delay(500);
    }
    /*--------Wrist servo---------*/
    if((RD3 == 1)&&(deg_wrist < 63))
    {
      deg_wrist++;
      Load_Wrist();
      delay(300);
    }
    else if((RD2 == 1)&&
      (deg_wrist > 0))
    {
      deg_wrist--;
      Load_Wrist();
      delay(300);
    }
    /*----Rotate Grip servo-------*/
    if ((RB6 == 1)&&
      (deg_r_grip < 63))
    {
      deg_r_grip++;
      Load_R_Grip();
      delay(500);
    }
    else if((RB7 == 1)&&
      (deg_r_grip > 0))
    {
      deg_r_grip--;
      Load_R_Grip();
      delay(500);
    }
    /*--------Grip servo----------*/
    if((RD1 == 1)&&(deg_grip < 55))
    {
      deg_grip++;
      Load_Grip();
      delay(300);
    }
    else if((RD0 == 1)&&
      (deg_grip > 15))
    {
      deg_grip--;
      Load_Grip();
      delay(300);
    }
  }
}


